#include "emb_topic.h"


typedef struct{
    emb_subscriber_ctrl_t *Next;
    uint8_t Id;
} emb_subscriber_ctrl_t;

emb_topic_ctrl_t *topic_head = NULL;
emb_memblk_ctrl_t topic_list_mem;

int emb_topic_init(emb_topic_ctrl_t* Topic,const char* TopicName,uint8_t* Buf,size_t BufSize)
{

    Topic->SubscriberList = NULL;
    Topic->TopicName = TopicName;
    emb_fifo_init(&Topic->Fifo, Buf, BufSize);

    EMB_PORT_DISABLE_IRQ();
    Topic->Next = topic_head;
    topic_head = Topic;
    EMB_PORT_ENABLE_IRQ();
    return 0;
}

int emb_topic_subscribe(emb_task_id_t Id,emb_topic_ctrl_t *Topic)
{
    emb_subscriber_ctrl_t *subscriber;
    subscriber = emb_memblk_alloc(&topic_list_mem);
    if(subscriber!=NULL)
    {
        subscriber->Id = Id;
        EMB_PORT_DISABLE_IRQ();
        subscriber->Next = Topic->SubscriberList;
        Topic->SubscriberList = subscriber;
        EMB_PORT_ENABLE_IRQ();
        return 0;
    }

    return -1;
}

int emb_topic_subscribe2(emb_task_id_t Id,const char* TopicName)
{
    emb_topic_ctrl_t *topic;

    if(topic!=NULL)
    {
        return emb_topic_subscribe(Id,topic);
    }

    return -1;
}

int emb_topic_unsubscribe(emb_task_id_t Id,emb_topic_ctrl_t *Topic)
{
    emb_subscriber_ctrl_t *cur;
}

int emb_topic_unsubscribe2(emb_task_id_t Id,const char* TopicName)
{
    emb_topic_ctrl_t *topic;

    if(topic!=NULL)
    {
        return emb_topic_unsubscribe(Id, topic);
    }

    return -1;
}

int emb_topic_msg_send(emb_topic_ctrl_t *Topic,const uint8_t* Buf,uint16_t Size)
{
    if(emb_fifo_remain(&Topic->Fifo)<(Size+sizeof(uint16_t)))
    {
        return -1;
    }

    EMB_PORT_DISABLE_IRQ();
    emb_fifo_push(&Topic->Fifo, &Size, sizeof(uint16_t));
    emb_fifo_push(&Topic->Fifo, Buf, Size);
    EMB_PORT_ENABLE_IRQ();
    return 0;
}

int emb_topic_msg_send2(const char* TopicName,const uint8_t* Buf,uint16_t Size)
{
    emb_topic_ctrl_t *topic;

    if(topic!=NULL)
    {
        return emb_topic_msg_send(topic, Buf, Size);
    }

    return -1;
}

void emb_topic_msg_recv(emb_topic_ctrl_t *Topic,const uint8_t** Buf,uint16_t* Size)
{
    *Size = emb_fifo_head(&Topic->Fifo);
    *Buf = (uint8_t *)emb_fifo_head(&Topic->Fifo) + 2;
}

void emb_topic_handler()
{
    emb_topic_ctrl_t *head=topic_head;
    emb_subscriber_ctrl_t *subscriber;
    uint16_t *size;
    while (head != NULL)
    {
        while(1)
        {
            size = (uint16_t*)emb_fifo_head(&head->Fifo);
            if(size==NULL)
            {
                break;
            }

            for (subscriber = head->SubscriberList; subscriber != NULL;subscriber=subscriber->Next)
            {
                task_table[subscriber->Id].Callback(subscriber->Id, EMB_EVENT_TOPIC_MSG);
            }
            head->Fifo.ReadIndex += sizeof(uint16_t)+*size;
        }
        head = head->Next;
    }
}
